
#include <agv_peripheral_controller/roller/roller_signal.h>
namespace peripheral_roller
{
    ///////////////////////////////CRollerSignal 实现///////////////////////////////////////////////
    // 默认构造函数
    CRollerSignal::CRollerSignal()
    {
        m_init = false;
    }

    // 析构函数
    CRollerSignal::~CRollerSignal()
    {
        m_subIoNew.shutdown();
    }

    // 是否初始化完成
    bool CRollerSignal::initFinish()
    {
        if (!m_init)
        {
            DEBUG_ERROR_OUT("RollerSignal init failed");
        }
        return m_init;
    }

    // 初始化函数
    bool CRollerSignal::init()
    {

        // 订阅输入信号话题/readInput
        m_subIoNew = m_priNh.subscribe("/readInputNew", 2, &CRollerSignal::readInputNewCallBack, this);
        m_subIoNewSim = m_priNh.subscribe("/readInputNewSim", 2, &CRollerSignal::readInputNewSimCallBack, this);

        m_outClient = m_priNh.serviceClient<agv_srvs::srv::OutputsNew>("/outputNew");

        // 初始化io key-名字
        agv_msgs::msg::PairTypeInt io;
        io.value = 0;
        // 复位按钮
        io.key = RESET_BTN;
        m_resetBtn.Update(io);
        ///////////////////////输入IO/////////////////////////////
        // 光通讯模块IO-DT
        io.key = OC_DATA_TRANSMISSION;
        m_ocDataT.Update(io);

        // 光通信信号 四路输入 1-4
        io.key = OC_INPUT1;
        m_ocInputs.push_back(io);

        io.key = OC_INPUT2;
        m_ocInputs.push_back(io);

        io.key = OC_INPUT3;
        m_ocInputs.push_back(io);

        io.key = OC_INPUT4;
        m_ocInputs.push_back(io);

        // 货物检测 四路输入 1-4
        io.key = CARGO_INPUT1;
        m_cargoInputs.push_back(io);

        io.key = CARGO_INPUT2;
        m_cargoInputs.push_back(io);

        io.key = CARGO_INPUT3;
        m_cargoInputs.push_back(io);

        io.key = CARGO_INPUT4;
        m_cargoInputs.push_back(io);

        // 辊筒正反转-手动旋钮
        io.key = SWITCH_FORWARD_ROLLER;
        m_forwardSwitch.Update(io);

        io.key = SWITCH_REVERSE_ROLLER;
        m_reverseSwitch.Update(io);

        ///////////////////////输出IO/////////////////////////////
        // 辊筒电源输出
        io.key = POWER_ROLLER;
        m_powerOutput = io;

        // 光通讯模块多路输出1-4
        io.key = OC_OUTPUT1;
        m_ocOutputs.push_back(io);

        io.key = OC_OUTPUT2;
        m_ocOutputs.push_back(io);

        io.key = OC_OUTPUT3;
        m_ocOutputs.push_back(io);

        io.key = OC_OUTPUT4;
        m_ocOutputs.push_back(io);

        // HMI -一键取货、卸货、暂停、结束虚拟IO
        io.key = HMI_PICK;
        INPUT_IO_TYPE tempIo;
        tempIo.Update(io);
        m_hmiIo[HmiSig::pick] = tempIo;

        io.key = HMI_DROP;
        tempIo.Update(io);
        m_hmiIo[HmiSig::drop] = tempIo;

        io.key = HMI_PAUSE;
        tempIo.Update(io);
        m_hmiIo[HmiSig::pause] = tempIo;

        io.key = HMI_FINISH;
        tempIo.Update(io);
        m_hmiIo[HmiSig::finish] = tempIo;

        // 虚拟oc输出io
        io.key = HMI_OC_OUT1;
        tempIo.Update(io);
        m_hmiIo[HmiSig::ocOu1] = tempIo;

        io.key = HMI_OC_OUT2;
        tempIo.Update(io);
        m_hmiIo[HmiSig::ocOu2] = tempIo;

        io.key = HMI_OC_OUT3;
        tempIo.Update(io);
        m_hmiIo[HmiSig::ocOu3] = tempIo;

        io.key = HMI_OC_OUT4;
        tempIo.Update(io);
        m_hmiIo[HmiSig::ocOu4] = tempIo;

        // 初始IO控制输出客户端
        m_outputIoTool.init(m_outClient);
        m_outputSimIoTool.init(m_outClient);

        m_init = true;
        return true;
    }

    std::size_t CRollerSignal::cargoInputSize()
    {
        return m_cargoInputs.size();
    }

    uint64_t CRollerSignal::cargoInputBits(bool positive)
    {
        uint64_t bits = 0;
        // 正序
        uint j = 0;
        int count = m_cargoInputs.size();

        if (positive)
        {
            for (int i = 0; i < count; i++)
            {
                bits = cargoInput(i) ? SET_BIT(bits, j) : CLEAR_BIT(bits, j);
                j++;
            }
        }
        else
        {
            // 倒叙
            for (int i = count - 1; i >= 0; i--)
            {
                bits = cargoInput(i) ? SET_BIT(bits, j) : CLEAR_BIT(bits, j);
                j++;
            }
        }

        return bits;
    }

    // 手动旋钮开关
    SwitchState CRollerSignal::switchBtn()
    {
        if (!initFinish())
        {
            return SwitchState::STOP;
        }

        bool forward = inputIo(m_forwardSwitch);
        bool reverse = inputIo(m_reverseSwitch);
        if (forward && reverse)
        {
            DEBUG_ERROR_OUT("roller motor switch btn error");
            return SwitchState::STOP;
        }

        return forward ? SwitchState::FORWARD : (reverse ? SwitchState::REVERSE : SwitchState::STOP);
    }

    // 检测某一路货物输入是否触发
    bool CRollerSignal::cargoInput(std::size_t index)
    {

        if (index >= m_cargoInputs.size())
        {
            DEBUG_ERROR_OUT("error index: cargoInput index=" << index);
            return false;
        }

        return inputIo(m_cargoInputs[index]);
    }

    // 检测光通信输入通道某一路是否触发
    bool CRollerSignal::ocInput(std::size_t index)
    {

        if (index >= m_ocInputs.size())
        {
            DEBUG_ERROR_OUT("error index: ocInput index=" << index);
            return false;
        }

        return inputIo(m_ocInputs[index]);
    }

    // 单独设置某一路光通讯输出
    bool CRollerSignal::ocOutput(std::size_t index, bool level)
    {
        if (index >= m_ocOutputs.size())
        {
            DEBUG_ERROR_OUT("error index: ocOutput index=" << index);
            return false;
        }

        auto &data = m_ocOutputs[index];
        data.value = level;

        IO_GROUP outputs;
        outputs.push_back(data);
        return m_outputIoTool.set(outputs);
    }

    // 辊筒电机驱动器电源设置
    bool CRollerSignal::powerSet(bool level)
    {
        m_powerOutput.value = level;
        IO_GROUP outputs;
        outputs.push_back(m_powerOutput);
        return m_outputIoTool.set(outputs);
    }

    // 货物装填
    bool CRollerSignal::loadStatus()
    {
        bool load = false;
        if (!initFinish())
        {
            return load;
        }

        for (auto &data : m_cargoInputs)
        {
            load |= inputIo(data);
        }

        return load;
    }

    // 光通讯数据通信正常且信号处于复位状态
    bool CRollerSignal::ocInputNormal()
    {
        bool ret = false;
        for (auto &data : m_ocInputs)
        {
            ret |= inputIo(data);
        }
        return !ret;
    }

    // 光通讯输出信号复位
    bool CRollerSignal::ocOutputReset()
    {
        for (auto &data : m_ocOutputs)
        {
            data.value = 0;
        }
        return m_outputIoTool.set(m_ocOutputs);
    }

    // 光通讯模式是否正常
    bool CRollerSignal::ocDataT()
    {
        return inputIo(m_ocDataT);
    }

    bool CRollerSignal::resetBtnPressed()
    {
        return inputIo(m_resetBtn);
    }

    // 获取虚拟hmi io
    bool CRollerSignal::getHmi(HmiSig index)
    {

        auto it = m_hmiIo.find(index);
        if (it == m_hmiIo.end())
        {
            DEBUG_ERROR_OUT("getHmi falied:index=" << (int)index);
            return false;
        }

        return inputIo(it->second);
    }

    // 输出仿真oc out信号
    bool CRollerSignal::simOcOutCycle()
    {
        if (!m_sim)
        {
            return false;
        }

        if (m_ocOutputs.size() < 4)
        {
            DEBUG_ERROR_OUT("m_ocOutputs is less 4");
            return false;
        }
        
        auto oc = m_ocOutputs;

        oc[0].value = getHmi(HmiSig::ocOu1);
        oc[1].value = getHmi(HmiSig::ocOu2);
        oc[2].value = getHmi(HmiSig::ocOu3);
        oc[3].value = getHmi(HmiSig::ocOu4);
        return m_outputSimIoTool.set(oc);
    }

    // 新更新输入IO对象
    void CRollerSignal::updateInutIoNew(const agv_msgs::msg::ReadInPutsNew &msg, INPUT_IO_TYPE &inputIoObj)
    {
        auto inputIo = inputIoObj.Get();
        std::vector<agv_msgs::msg::PairTypeInt> inputs_msg = msg.array;
        for (auto &data : inputs_msg)
        {
            if (data.key == inputIo.key)
            {
                inputIoObj.Update(data);
                // DEBUG_OUT("updateInutIoNew:inputIo.key=" << inputIo.key << ",data.value=" << data.value);
                break;
            }
        }
    }

    // 获取IO输入信号回调函数 io topic话题频率30hz
    void CRollerSignal::readInputNewCallBack(const agv_msgs::msg::ReadInPutsNew &msg)
    {
        // 复位按钮
        updateInutIoNew(msg, m_resetBtn);

        // 获取举升上下限位IO信号 并更新数据的时间戳
        updateInutIoNew(msg, m_ocDataT);

        // 仿真io启用，此处则不更新io状态
        if (!m_sim)
        {
            for (auto &data : m_ocInputs)
            {
                updateInutIoNew(msg, data);
            }

            for (auto &data : m_cargoInputs)
            {
                updateInutIoNew(msg, data);
            }
        }

        // 获取手动旋钮开关的状态
        updateInutIoNew(msg, m_forwardSwitch);
        updateInutIoNew(msg, m_reverseSwitch);
    }

    // 获取IO输入信号回调函数 io topic话题频率30hz
    void CRollerSignal::readInputNewSimCallBack(const agv_msgs::msg::ReadInPutsNewSim &msg)
    {

        // 仿真io启用，此处更新虚拟io状态
        if (msg.open)
        {
            // 更新仿真光通信输入
            for (auto &data : m_ocInputs)
            {
                updateInutIoNew(msg.msg, data);
            }
            // 更新仿真光通信货物光电开关
            for (auto &data : m_cargoInputs)
            {
                updateInutIoNew(msg.msg, data);
            }
        }

        // 更新虚拟hmi io:一键取送货+ocOut1~4(只有在sim下才生效)
        for (auto &data : m_hmiIo)
        {
            updateInutIoNew(msg.msg, data.second);
        }

        m_sim = msg.open;

        ROS_INFO_STREAM_THROTTLE(0.5, "IoSimCallBack:" << msg);
    }

}; // end namespace
